/**
 * @file SmallObsPlanner.h
 * @author  
 * @brief 小障碍物行为规划
 * @version 1.0
 * @date 2020-07-23
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#pragma once
#include "data_struct/robot/Pose.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller
{

    class SmallObsPlanner
    {
        public :
        SmallObsPlanner(){};

        void run(double &linear_vel, double &angular_vel);
        void TurnToAngle(double &linear_vel, double &angular_vel,double &dst_angle);
        void pid_adjustment(double &linear_vel, double &angular_vel);

        public :
        std::vector<std::pair<bool, PointF>> small_obs;
        PoseF last_pose;
        PointF mean_pose_local;
        double global_goal_angle;
        double total_angle;
        double obs_len;
        double turn_radius;
        double last_angle;
        double turn_sum;
        double last_dis_p;
        double aim_dist;
        double point_count;
        double obs_dis;
        double angular_vel_aim;
        bool init_angle=false;
        bool finished_turn=false;
        bool finished=true;

    };
    //static 
    extern SmallObsPlanner small_obs_Planner;
}